﻿// 标准库或者第三方库

// 自定义头文件
#include "template_problem03.hxx"

// ACIS
#include "acis/acis.hxx"
#include "acis/geom_utl.hxx"

#include <cmath>
#include <algorithm>
#include <acis/acistol.hxx>
#include <acis/elldef.hxx>
#include <array>

//#define GME_ELLI
#define SDL_ELLI

// ************************************************************

SPAinterval xy_ell::xy_ell_param_range() const {
#ifdef GME_ELLI
    return SPAinterval{ -M_PI, M_PI };
#endif // GME_ELLI

#ifdef SDL_ELLI
    double a,b;
    a = this->major_axis.len();
    b = a * radius_ratio;
	return SPAinterval{ 0, 1 / 1e13};
#endif // SDL_ELLI

    // return SPAinterval();
}

void xy_ell::xy_ell_evaluate(double val, SPAposition& pos, SPAvector& first, SPAvector& second) const {

#ifdef GME_ELLI
    // gme版本，目前用于测试
	SPAvector minor_axis = normal * major_axis * radius_ratio;
	double cost = std::clamp(cos(val), -1.0, 1.0);
	double sint = std::clamp(sin(val), -1.0, 1.0);
	if (&pos != (SPAposition*)NULL_REF) {
		pos = centre + cost * major_axis + sint * minor_axis;
	}
	if (&first != (SPAvector*)NULL_REF) {
		first = cost * minor_axis - sint * major_axis;
	}
	if (&second != (SPAvector*)NULL_REF) {
		second = -cost * major_axis - sint * minor_axis;
	}
#endif // GME_ELLI

#ifdef SDL_ELLI

    double a = major_axis.len();  // 长轴
    double b = a * radius_ratio;  // 短轴

    SPAvector x_axis = major_axis / a;
    SPAvector y_axis = normal * x_axis;
    //double L = val;
    double L = val *  4 * (a + b) * 1e13;

    if(fabs(radius_ratio - 1) < 1e-8) {
        double h1, h2, h3, h4;
        double x, y, dx, dy, ddx, ddy;  // 结果的坐标
        double a2 = 2 * a;
        double aSquare = a * a;
        int L_divide_a = L / a;
        double temp1;
        double root;
        if(&pos != nullptr) {
            if(L_divide_a < 2) {
                double L_subtract_a = L - a;
                h1 = -L * L + a2 * L + aSquare;
                root = sqrt(h1);
                x = (-L_subtract_a + root) / 2;
                y = (L_subtract_a + root) / 2;
                if(&first != nullptr) {
                    temp1 = L_subtract_a / (2 * root);
                    dx = -temp1 - 0.5;
                    dy = -temp1 + 0.5;
                    first = x_axis * (dx) + y_axis * (dy);
                }
                if(&second != nullptr) {
                    temp1 = aSquare / (root * h1);
                    ddx = -temp1;
                    ddy = -temp1;
                    second = x_axis * (ddx) + y_axis * (ddy);
                }
            } else if(L_divide_a < 4) {
                double L_subtract_3a = L - 3 * a;
                h2 = -L * L + 6 * a * L - 7 * aSquare;
                root = sqrt(h2);
                x = (-L_subtract_3a - root) / 2;
                y = (-L_subtract_3a + root) / 2;
                if(&first != nullptr) {
                    temp1 = L_subtract_3a / (2 * root);
                    dx = temp1 - 0.5;
                    dy = -temp1 - 0.5;
                    first = x_axis * (dx) + y_axis * (dy);
                }
                if(&second != nullptr) {
                    temp1 = aSquare / (root * h2);
                    ddx = temp1;
                    ddy = -temp1;
                    second = x_axis * (ddx) + y_axis * (ddy);
                }
            } else if(L_divide_a < 6) {
                double L_subtract_5a = L - 5 * a;
                h3 = -L * L + 10 * a * L - 23 * aSquare;
                root = sqrt(h3);
                x = (L_subtract_5a - root) / 2;
                y = (-L_subtract_5a - root) / 2;
                if(&first != nullptr) {
                    temp1 = L_subtract_5a / (2 * root);
                    dx = temp1 + 0.5;
                    dy = temp1 - 0.5;
                    first = x_axis * (dx) + y_axis * (dy);
                }
                if(&second != nullptr) {
                    temp1 = aSquare / (root * h3);
                    ddx = temp1;
                    ddy = temp1;
                    second = x_axis * (ddx) + y_axis * (ddy);
                }
            } else {
                double L_subtract_7a = L - 7 * a;
                h4 = -L * L + 14 * a * L - 47 * aSquare;
                root = sqrt(h4);
                x = (L_subtract_7a + root) / 2;
                y = (L_subtract_7a - root) / 2;
                if(&first != nullptr) {
                    temp1 = L_subtract_7a / (2 * root);
                    dx = -temp1 + 0.5;
                    dy = temp1 + 0.5;
                    first = x_axis * (dx) + y_axis * (dy);
                }
                if(&second != nullptr) {
                    temp1 = aSquare / (root * h4);
                    ddx = -temp1;
                    ddy = temp1;
                    second = x_axis * (ddx) + y_axis * (ddy);
                }
            }

            SPAvector v = x_axis * (x) + y_axis * (y);

            pos = {v.x() + centre.x(), v.y() + centre.y(), v.z() + centre.z()};

        }

    } else {
        double a_add_b = a + b;
        int L_divide_aaddb = L / a_add_b;
        double a_multiply_b = a * b;
        double aSquare = a * a;
        double bSquare = b * b;
        double g = aSquare + bSquare;
        double h1, h2, h3, h4;
        double x, y, dx, dy, ddx, ddy;  // 结果的坐标
        double temp1, temp2, root;      // 记录中间变量

        if(&pos != nullptr) {
            if(L_divide_aaddb < 1) {
                h1 = -L * L + 2 * a * L + bSquare;
                root = sqrt(h1);
                temp1 = a_multiply_b * root;
                temp2 = L - a;
                x = (-aSquare * temp2 + temp1) / g;
                y = (bSquare * temp2 + temp1) / g;
                if(&first != nullptr) {
                    temp1 = temp2 / root;
                    dx = -(aSquare + a_multiply_b * temp1) / g;
                    dy = (bSquare - a_multiply_b * temp1) / g;
                    first = x_axis * (dx) + y_axis * (dy);
                }
                if(&second != nullptr) {
                    temp1 = a_multiply_b / (root * h1);
                    ddx = -temp1;
                    ddy = -temp1;
                    second = x_axis * (ddx) + y_axis * (ddy);
                }
            } else if(L_divide_aaddb < 2) {
                double a_add_b2 = a + 2 * b;
                h2 = -L * L + 2 * L * a_add_b2 - 3 * bSquare - 4 * a_multiply_b;
                root = sqrt(h2);
                temp1 = a_multiply_b * root;
                temp2 = L - a_add_b2;
                x = (-aSquare * temp2 - temp1) / g;
                y = (-bSquare * temp2 + temp1) / g;
                if(&first != nullptr) {
                    temp1 = (L - a_add_b2) / root;
                    dx = -(aSquare - a_multiply_b * temp1) / g;
                    dy = -(bSquare + a_multiply_b * temp1) / g;
                    first = x_axis * (dx) + y_axis * (dy);
                }
                if(&second != nullptr) {
                    temp1 = a_multiply_b / (root * h2);
                    ddx = temp1;
                    ddy = -temp1;
                    second = x_axis * (ddx) + y_axis * (ddy);
                }
            } else if(L_divide_aaddb < 3) {
                double a3_add_b2 = 3 * a + 2 * b;
                h3 = -L * L + 2 * L * a3_add_b2 - 8 * aSquare - 12 * a_multiply_b - 3 * bSquare;
                root = sqrt(h3);
                temp1 = a_multiply_b * root;
                temp2 = L - a3_add_b2;
                x = (aSquare * temp2 - temp1) / g;
                y = (-bSquare * temp2 - temp1) / g;
                if(&first != nullptr) {
                    temp1 = (L - a3_add_b2) / root;
                    dx = (aSquare + a_multiply_b * temp1) / g;
                    dy = -(bSquare - a_multiply_b * temp1) / g;
                    first = x_axis * (dx) + y_axis * (dy);
                }
                if(&second != nullptr) {
                    temp1 = a_multiply_b / (root * h3);
                    ddx = temp1;
                    ddy = temp1;
                    second = x_axis * (ddx) + y_axis * (ddy);
                }
            } else {
                double a3_add_b4 = 3 * a + 4 * b;
                h4 = -L * L + 2 * L * a3_add_b4 - 8 * aSquare - 24 * a_multiply_b - 15 * bSquare;
                root = sqrt(h4);
                temp1 = a_multiply_b * root;
                temp2 = L - a3_add_b4;
                x = (aSquare * temp2 + temp1) / g;
                y = (bSquare * temp2 - temp1) / g;
                if(&first != nullptr) {
                    temp1 = (L - a3_add_b4) / root;
                    dx = (aSquare - a_multiply_b * temp1) / g;
                    dy = (bSquare + a_multiply_b * temp1) / g;
                    first = x_axis * (dx) + y_axis * (dy);
                }
                if(&second != nullptr) {
                    temp1 = a_multiply_b / (root * h4);
                    ddx = -temp1;
                    ddy = temp1;
                    second = x_axis * (ddx) + y_axis * (ddy);
                }
            }
            // posi = SPAposition(x, y, 0);
            SPAvector v = x_axis * (x) + y_axis * (y);

            // posi = {v.x(), v.y(), v.z()};
            pos = {v.x() + centre.x(), v.y() + centre.y(), v.z() + centre.z()};

        }
    }
#endif // SDL_ELLI
	
}

double xy_ell::xy_ell_param(SPAposition const& pos) const {
#ifdef GME_ELLI
    // gme版本，目前用于测试
	SPAvector real_minor_axis = normalise(normal * major_axis) * major_axis.len() * radius_ratio;                               // 计算短轴
	auto trans_to_origin = coordinate_transf(centre, normalise(major_axis), normalise(real_minor_axis)).inverse();  // 由世界坐标系转换到椭圆坐标系的矩阵
	const SPAposition ori_name = pos * trans_to_origin;                                                      // 空间中点在椭圆下的坐标
	const SPAposition ori_centre(0, 0, 0);                                                                                          // 椭圆原点
	const SPAposition ori_pos_proj(ori_name.x(), ori_name.y(), 0);                                                                  // ori_name在椭圆平面内的投影点
	const SPAvector ori_major_axis = SPAunit_vector(1, 0, 0) * major_axis.len();
	const SPAvector ori_minor_axis = SPAunit_vector(0, 1, 0) * real_minor_axis.len();
	const SPAunit_vector ori_normal(0, 0, 1);

	if (ori_pos_proj == ori_centre) {
		return M_PI / 2;
	}
	// eval()函数计算公式:posi = centre + major_axis * cos(val) + real_minor_axis * sin(val)
	const auto displacement = ori_pos_proj - ori_centre;  // 椭圆中心到给定点的向量
	double _cos_u = displacement.x() / displacement.len();
	double _sin_u = displacement.y() / displacement.len();
	double res = 0;
	if (_cos_u != 0) {
		bool u_positive;
		const double& f = radius_ratio;
		// 以下的情况判断是为了提高计算精度
		if (abs(_cos_u) < SPAresabs || (abs(_cos_u) > SPAresabs && (abs(_sin_u) > SPAresabs || is_zero_mch(_sin_u)))) {              // 这种情况下通过cos_u计算param
			u_positive = (displacement.y() >= 0);                                                                                   // 判断点在y轴正向还是负向，用于判断arccos正负
			double real_cos_u = std::clamp(sqrt(f * f * _cos_u * _cos_u / (f * f * _cos_u * _cos_u + _sin_u * _sin_u)), 0.0, 1.0);  //_cos_u为椭圆长轴对应圆的余弦值，椭圆的real_cos_u根据推导而来
			real_cos_u = (_cos_u > 0) ? real_cos_u : -real_cos_u;
			res = (u_positive) ? acos(real_cos_u) : -acos(real_cos_u);
		}
		else {                                                                                                            // 否则通过sin_u计算param
			u_positive = (displacement.x() >= 0);                                                                           // 判断点在x轴正向还是负向
			double real_sin_u = std::clamp(sqrt(_sin_u * _sin_u / (f * f * _cos_u * _cos_u + _sin_u * _sin_u)), 0.0, 1.0);  //_sin_u为椭圆长轴对应圆的余弦值，椭圆的real_sin_u根据推导而来
			real_sin_u = (_sin_u > 0) ? real_sin_u : -real_sin_u;
			res = (u_positive) ? asin(real_sin_u) : (real_sin_u >= 0 ? M_PI - asin(real_sin_u) : -M_PI - asin(real_sin_u));
		}
		if (abs(res - M_PI) < SPAresabs / 1000) res = -M_PI;
	}
	else {  // 如果点在椭圆y轴上的情况
		SPAposition minor_positive_pos = ori_centre + ori_minor_axis;
		SPAposition minor_negative_pos = ori_centre + (-ori_minor_axis);
		SPAposition temp_pos = normalise(displacement) * real_minor_axis.len() + ori_centre;
		if (temp_pos == minor_positive_pos)
			res = M_PI / 2;
		else if (temp_pos == minor_negative_pos)
			res = -M_PI / 2;
	}
	return res;
	//return 0;
#endif // GME_ELLI

#ifdef SDL_ELLI
    double a = major_axis.len();                 // 长轴
    double b = major_axis.len() * radius_ratio;  // 短轴

    SPAvector x_axis = major_axis / a;
    SPAvector y_axis = normal * x_axis;

    SPAvector posi = SPAvector(pos.x(), pos.y(), pos.z());

    SPAvector v(centre.x(), centre.y(), centre.z());

    double x = (posi - v) % x_axis;
    double y = (posi - v) % y_axis;

    if(true) {
        double c = x * x / (a * a) + y * y / (b * b);

        double scale;
        if(fabs(c - 1.0) > 1e-8) {
            scale = sqrt(c);
            x = x / scale;
            y = y / scale;
        }
    }

    double a_add_b = a + b;

    double peirod = (a_add_b)*4;

    double q1 = x >= 0 ? 1.0 : -1.0;

    double q2 = y >= 0 ? 1.0 : -1.0;

    double L = q1 * y - q2 * x + 2 * a_add_b - q2 * (a_add_b + b * q1);

    if (L - 4 * (a + b) > -1e-8)
    {
        L -= 4 * (a + b);
    }


    return L / (4 * (a + b)) / 1e13 ;
    //return L ;
#endif // SDL_ELLI
}

SPAvector xy_ell::xy_ell_point_curvature(SPAposition const& pos) {
#ifdef GME_ELLI
    // gme版本，目前用于测试
	SPAvector real_minor_axis = normalise(normal * major_axis) * major_axis.len() * radius_ratio;
	const double t = xy_ell_param(pos);
	double a = major_axis.len();
	double b = real_minor_axis.len();
	SPAvector deriv;
	SPAposition pos_t;
    // pos_proj = (530.98062604559436,-2620.0432270610854,2620.0432270610863)
	SPAposition pos_proj = proj_pt_to_plane(pos, centre, normal);
	xy_ell_evaluate(t, pos_t, deriv);
	double factor = (pos_proj - centre).len() / (pos_t - centre).len();  // factor为原椭圆需要扩大的比例
	// 椭圆参数方程曲率计算公式
	a *= factor, b *= factor;

	const double cur = a * b / pow((a * a * sin(t) * sin(t) + b * b * cos(t) * cos(t)), 1.5);
	if (std::isnan(cur)) {
		return { 0, 0, 0 };
	}
	// normal * deriv计算曲率方向
	SPAvector dir = normal * deriv;
	if (dir % (centre - pos) < 0) dir = -dir;
	return cur * normalise(dir);

	// return SPAvector();
#endif // GME_ELLI

#ifdef SDL_ELLI

    SPAvector deriv1,deriv2;
	SPAposition pos_t;
    SPAvector real_minor_axis = normalise(normal * major_axis) * major_axis.len() * radius_ratio;

    SPAposition vec1 = SPAposition(major_axis.x(),major_axis.y(),major_axis.z());
    SPAposition vec2 = SPAposition(real_minor_axis.x(),real_minor_axis.y(),real_minor_axis.z());
    SPAposition proj_vec1 = proj_pt_to_plane(vec1, centre, normal);
    SPAposition proj_vec2 = proj_pt_to_plane(vec2, centre, normal);
    double a = sqrt(proj_vec1.x() * proj_vec1.x() + proj_vec1.y() * proj_vec1.y() + proj_vec1.z() * proj_vec1.z());
    double b = sqrt(proj_vec2.x() * proj_vec2.x() + proj_vec2.y() * proj_vec2.y() + proj_vec2.z() * proj_vec2.z());
    SPAposition pos_proj = proj_pt_to_plane(pos, centre, normal);

    // 计算旋转轴
    SPAvector target = {0, 0, 1};
    SPAvector rotationAxis = normal * target;
    rotationAxis = normalise(rotationAxis);

    // 计算旋转角度
    double cosTheta  = -normal % target; // 点积
    double sinTheta = sqrt(1 - cosTheta * cosTheta);

    
    // 待旋转坐标系
    SPAvector axis_x = {1,0,0};
    SPAvector axis_y = {0,1,0};
    SPAvector rotated_x = axis_x * cosTheta + (rotationAxis * axis_x) * sinTheta + rotationAxis * (rotationAxis % axis_x) * (1 - cosTheta);
    SPAvector rotated_y = axis_y * cosTheta + (rotationAxis * axis_y) * sinTheta + rotationAxis * (rotationAxis % axis_y) * (1 - cosTheta);
    SPAmatrix M = {{rotated_x.x(),rotated_y.x(),normal.x()},{rotated_x.y(),rotated_y.y(),normal.y()},{rotated_x.z(),rotated_y.z(),normal.z()}};
    SPAposition result = M * pos_proj;

    double factor = sqrt(result.x() * result.x() / (a * a) + result.y() * result.y() / (b * b) );
    major_axis = factor * major_axis;
    const double t = xy_ell_param(pos);

    xy_ell_evaluate(t, pos_t, deriv1, deriv2);

    major_axis = major_axis / factor;

      double v = deriv1.x() * deriv1.x() + deriv1.y() * deriv1.y() + deriv1.z() * deriv1.z();

	// normal * deriv计算曲率方向
	SPAvector dir = normal * deriv1;
	if (dir % (centre - pos) < 0) dir = -dir;
	//return cur * normalise(dir);

    SPAvector vec = deriv1 * deriv2 ;
    double cur = sqrt(vec.x() * vec.x() + vec.y() * vec.y() + vec.z() * vec.z())/ (sqrt(v) * v);
    return cur * normalise(dir);

#endif // SDL_ELLI

}

SPAunit_vector xy_ell::xy_ell_point_direction(SPAposition const& pos) const {
#ifdef GME_ELLI
    // gme版本，目前用于测试
	const double t = xy_ell_param(pos);
	SPAvector first_deriv;
	xy_ell_evaluate(t, *(SPAposition*)nullptr, first_deriv);
	return normalise(first_deriv);
	//return SPAunit_vector();
#endif // GME_ELLI


#ifdef SDL_ELLI
	const double t = xy_ell_param(pos);
	SPAvector first_deriv;
    SPAposition point;
	xy_ell_evaluate(t, point, first_deriv);
	return normalise(first_deriv);
#endif // SDL_ELLI


}
